package com.testcolision.simulation;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.math.MathUtils;
import com.badlogic.gdx.math.Vector2;
import com.badlogic.gdx.utils.Logger;

public class Wheel {
	Logger l= new Logger("log", Logger.INFO);
	private Vector2 m_forwardAxis = new Vector2();
	private Vector2 m_sideAxis = new Vector2();
	private float m_wheelTorque, m_wheelSpeed, m_wheelInertia, m_wheelRadius;
	private Vector2 m_Position = new Vector2();

	public Wheel(Vector2 position, float radius) {
		
		m_Position.set(position);
		SetSteeringAngle(0);
		m_wheelSpeed = 0;
		m_wheelRadius = radius;
		m_wheelInertia = radius * radius; // fake value
	}

	public void SetSteeringAngle(float newAngle) {
		// Matrix3 mat = new Matrix3();
		// PointF[] vectors = new PointF[2];
		//
		// //foward vector
		// vectors[0].X = 0;
		// vectors[0].Y = 1;
		// //side vector
		// vectors[1].X = -1;
		// vectors[1].Y = 0;
		//
		// mat.Rotate(newAngle / (float)Math.PI * 180.0f);
		// mat.TransformVectors(vectors);

		m_forwardAxis = new Vector2(0, -1).rotate(newAngle * MathUtils.radiansToDegrees);
		m_sideAxis = new Vector2(1, 0).rotate(newAngle * MathUtils.radiansToDegrees);
	}

	public void AddTransmissionTorque(float newValue) {
		m_wheelTorque += newValue;
	}

	public float GetWheelSpeed() {
		return m_wheelSpeed;
	}

	public Vector2 GetAttachPoint() {
		return m_Position.cpy();
	}

	public Vector2 CalculateForce(Vector2 relativeGroundSpeed, float timeStep) {
		// calculate speed of tire patch at ground
		Vector2 patchSpeed = m_forwardAxis.cpy().mul(m_wheelSpeed).mul(m_wheelRadius);

		// get velocity difference between ground and patch
		Vector2 velDifference = relativeGroundSpeed.cpy().sub(patchSpeed);

		// project ground speed onto side axis
		// float forwardMag = 0;
		// Vector2 sideVel = velDifference.Project(m_sideAxis);
		// Vector2 forwardVel = velDifference.Project(m_forwardAxis,
		// out forwardMag);

		float dotDistance = velDifference.cpy().dot(m_sideAxis);
		Vector2 sideVel = m_sideAxis.cpy().mul(dotDistance);
		float forwardMag = velDifference.cpy().dot(m_forwardAxis);
		float forwardMagFriction = relativeGroundSpeed.dot(m_forwardAxis);
		
		Vector2 forwardVel = m_forwardAxis.cpy().mul(forwardMag);

		// calculate super friction forces
		Vector2 superFrictionForce = new Vector2().set(m_forwardAxis).mul(-1).mul(forwardMagFriction); 

		// calculate response force
		Vector2 responseForce = new Vector2().set(sideVel.mul(-1).mul(2.0f));
		responseForce.sub(forwardVel).add(superFrictionForce);

		// calculate torque on wheel
		m_wheelTorque += forwardMag * m_wheelRadius;
		l.info("forwardMag"+forwardMag );
		l.info("torque: "+m_wheelTorque ); 
		l.info("responseForce: "+responseForce ); 	
		// integrate total torque into wheel
		m_wheelSpeed += m_wheelTorque / m_wheelInertia * timeStep;

		// clear our transmission torque accumulator
		m_wheelTorque = 0;
	 
 
		// return force acting on body
		return responseForce;
	}
}
